#pragma once
#include "I2CDevice.hpp"

class MPU6500 {
public:
    /**
     * @brief Constructs an MPU6500 object connected to the specified I2C device.
     *
     * @param i2cDevice Reference to the I2CDevice object that represents the I2C connection
     *                  to the MPU6500 sensor.
     */
    MPU6500(I2CDevice& i2cDevice);

    /**
     * @brief Destructor for the MPU6500 class.
     *
     * This is a default destructor that cleans up any resources held by the
     * MPU6500 object. It does not perform any specific actions.
     */
    ~MPU6500() = default;

    /**
     * @brief Initializes the MPU6500 sensor.
     *
     * This function initializes the MPU6500 sensor by sending configuration
     * commands over I2C. It sets up the sensor for data acquisition and ensures
     * that it is ready to start providing acceleration and gyroscope data.
     *
     * @return true if the initialization was successful, false otherwise.
     */
    bool initialize();

    /**
     * @brief Calibrates the MPU6500 sensor.
     *
     * This function calibrates the MPU6500 sensor by collecting data over a specified number of samples
     * and adjusting the zero offsets for the accelerometer and gyroscope to minimize bias.
     *
     * @param samples Number of samples to collect for calibration. Default is 200.
     */
    void calibrate(int samples = 200);

    /**
     * @brief Reads data from the MPU6500 sensor.
     *
     * This function reads raw acceleration and gyroscope data from the MPU6500 sensor.
     *
     * @param ax Reference to a float where the X-axis acceleration value will be stored.
     * @param ay Reference to a float where the Y-axis acceleration value will be stored.
     * @param az Reference to a float where the Z-axis acceleration value will be stored.
     * @param gx Reference to a float where the X-axis gyroscope value will be stored.
     * @param gy Reference to a float where the Y-axis gyroscope value will be stored.
     * @param gz Reference to a float where the Z-axis gyroscope value will be stored.
     */
    void readSensor(float& ax, float& ay, float& az, float& gx, float& gy, float& gz);

private:
    /**
     * Combines two bytes into a 16-bit integer.
     *
     * @param high The high byte of the data.
     * @param low The low byte of the data.
     * @return A 16-bit integer combining the high and low bytes.
     */
    int16_t combineBytes(uint8_t high, uint8_t low);

private:
    I2CDevice& i2c; /*!< I2C device used for communication with the MPU6500 sensor. */

    float accel_scale; /*!< Scale factor for acceleration data. */
    float gyro_scale; /*!< Scale factor for gyroscope data. */

    float accel_offset[3] = {0}; /*!< Offset values for acceleration data. */
    float gyro_offset[3] = {0}; /*!< Offset values for gyroscope data. */
};
